/*需求：1.解析雷达数据

数据类型：
波特率 230400/

FA A0  BB 0B    E9 04 AC 03 0D 00   37 05 E2 04 0D 00   1E 05 97 04 0D 00   97 05 51 04 0D 00   0A 02 00 00 0D 14    A6 01 00 00 10 04   16  16 
FA A1  B1 0B   B5 06 00 00  11 08   5E 05 56 09 17 00   5A 07 00 00 1B 00   78 07 00 00 1F 00   B3 10 00 00 23 00    91 01 00 0A 0B 00   54  54 
头 索  速度     强度         距离   预留 	强度   距离 预留	强度   距离 预留    强度   距离 预留	 强度   距离 预留	  强度   距离 预留	  校验

解析流程

esp32一次读取82字节数据存储数组，从数据里面挑选FA验证开头，A0验证首位，验证校验和，取出6个距离数据

写一个esp32串口解析360度激光雷达的程序，
雷达数据为FA开头，第二位为A0-DB共60组数据，每组数据共计42字节，每组数据FA开头，第2字节A0为角度值(范围A0-DB共60组)，
第7/8字节为第一度距离（低位在前高位在后），
第13/14字节为第二度距离（低位在前高位在后），
第19/20字节为第三度距离（低位在前高位在后），
第25/26字节为第四度距离（低位在前高位在后），
第31/32字节为第五度距离（低位在前高位在后），
第37/38字节为第六角度距离（低位在前高位在后），
第41/42字节为校验和（低位在前高位在后）
计算校验和方法（校验和=0xFF减去所有数据相加）

数据处理为避障做准备


将数据分为四组，
其中330度到30度之间数据作为第一组，其中60度到120度之间数据作为第二组，
其中150度到210度之间数据作为第三组，其中240度到300度之间数据作为第四组，

判断每组数据中是否有小于100或大于3500的数据，如果有则将此数据丢弃

取每组数据的平均值分别为LDP(1234)，

*/
#include <HardwareSerial.h>

// 新增临时存储和计数器
int tempGroupMin[4] = {351, 351, 351, 351};  // 临时存储每轮循环的最小值
int packetCounter = 0;                         // 数据包计数器

const uint8_t PACKET_SIZE = 42;
const uint8_t MEASUREMENTS_PER_PACKET = 6;
const uint8_t TOTAL_ANGLES = 60; // 计算总角度数（60个）

int groupMin[4] = {351, 351, 351, 351};
int testAngle;
int testValue;


HardwareSerial LidarSerial(2);

void setup() {
  Serial.begin(230400);
  LidarSerial.begin(230400, SERIAL_8N1, 16, 17);
  Serial.println("Lidar on");
  LidarSerial.print("b");
  delay(2000);
  Serial.println("Lidar Parser Ready");
}

void loop() {
  static uint8_t buffer[PACKET_SIZE];
  static uint8_t readPos = 0;

  while (LidarSerial.available()) {
    uint8_t data = LidarSerial.read();

    if (readPos == 0 && data != 0xFA) continue;
    buffer[readPos++] = data;

    if (readPos >= PACKET_SIZE) {
      if (buffer[0] == 0xFA) {
        parsePacket(buffer);
      }
      readPos = 0;
    }
  }
}

void parsePacket(uint8_t* packet) {
  if (!validateAngle(packet[1])) {
    Serial.println("ERR: Invalid angle");
    return;
  }

  if (!validateChecksum(packet)) {
    Serial.println("ERR: Checksum failed");
    return;
  }

  processMeasurements(packet);
  
  // 更新计数器并检查是否完成完整循环
  if (++packetCounter >= TOTAL_ANGLES) {
    // 更新全局最小值并重置临时存储
    for (int i = 0; i < 4; i++) {
      groupMin[i] = tempGroupMin[i];
      tempGroupMin[i] = 354; // 重置为初始值
    }
    
    // 输出最终结果（仅在完整循环后执行一次）
    printMinValues();
    
    packetCounter = 0; // 重置计数器
  }
}

bool validateAngle(uint8_t angleByte) {
  return (angleByte >= 0xA0) && (angleByte <= 0xDB);
}

bool validateChecksum(uint8_t* packet) {
  uint32_t sum = 0;
  for (int i = 0; i < PACKET_SIZE - 2; i++) {
    sum += packet[i];
  }

  uint8_t expected = 0xFF - (sum & 0xFF);
  uint16_t received = (packet[PACKET_SIZE - 2] << 8) | packet[PACKET_SIZE - 1];

  return (received & 0xFF) == expected;
}

void processMeasurements(uint8_t* packet) {
  uint8_t angleIndex = packet[1] - 0xA0;
  uint16_t measurements[MEASUREMENTS_PER_PACKET];
  
  for (int i = 0; i < MEASUREMENTS_PER_PACKET; i++) {
    uint8_t offset = 6 + i * 6;
    measurements[i] = (packet[offset]) | (packet[offset + 1] << 8);
  }

  testAngle = angleIndex;
  for (int i = 0; i < MEASUREMENTS_PER_PACKET; i++) {
    testValue = measurements[i] / 10;
    sjcl();
  }
}

bool processSensorData(int angle, int value, int* group) {
    // 角度分组判断
  if ((angle >= 0 && angle <= 4)|| (angle >= 56 && angle <= 59)) {
    *group = 1;
  } else if (angle >= 11 && angle <= 19) {
    *group = 2;
  } else if (angle >= 26 && angle <= 34) {
    *group = 3;
  } else if (angle >= 41 && angle <= 49) {
    *group = 4;
  } else {
    *group = 0;  // 不属于任何有效分组
    return false;
  }
  // 数据有效性检查
  if (value < 10 || value > 350) {
    return false;
  }
  return true;
}

void sjcl() {
  int currentGroup;
  if (processSensorData(testAngle, testValue, &currentGroup)) {
    if (currentGroup >= 1 && currentGroup <= 4) {
            int index = currentGroup - 1;
      if (testValue < tempGroupMin[index]) { // 更新临时存储
        tempGroupMin[index] = testValue;
      }
    }
  }
}

// 新增：统一输出函数（仅在完整循环后调用）
void printMinValues() {
  Serial.print("FF-");
  Serial.print(groupMin[0]);
  Serial.print(" RR-");
  Serial.print(groupMin[1]);
  Serial.print(" BB-");
  Serial.print(groupMin[2]);
  Serial.print(" LL-");
  Serial.println(groupMin[3]);
}